home *** CD-ROM | disk | FTP | other *** search
/ Aminet 6 / Aminet 6 - June 1995.iso / Aminet / misc / sci / RARS_Amiga_2.lha / RARS / arelys.cpp < prev    next >
Encoding:
C/C++ Source or Header  |  1995-03-31  |  6.3 KB  |  175 lines
  1. // ARELYS.CPP - A robot "driver" for RARS, by M. Timin, February, 1995
  2. // ver. 0.39 3/6/95
  3.  
  4. #include <string.h>
  5. #include <stdlib.h>
  6. #include <math.h>
  7. #include "car.h"
  8.  
  9. const double CORN_SPD_CON = 5.7;   // determines how fast to take corners
  10. const double BIAS = 0.05;          // estimate of alpha when cornering
  11. const double STEER_GAIN = 0.7;    // servo gain, for staying in "lane"
  12. const double STEER_DAMP = 2.0;     // servo damping, to prevent "weaving"
  13. const double SLIP_LIM = 4.0;     // maximum wheel slip, ft/sec, in wheel_slip()
  14. const double SLIP_CON = 199.0;    //
  15. const double SHARP_TURN = .1;    // change in alpha when attempting to pass
  16. const int PASSING_TIME = 70;     // time to stay in passing maneuver, counts
  17. const double CUSHION = 10.0;     // feet from inside rail to shoot for
  18. const double CRIT_WIDTHS = 2.9;  // length in widths meaning nearing corner
  19. const double BRAK_ACCEL = 23.0;  // braking acceleration, ft/sec sq.
  20.  
  21. extern char* glob_name;       // The name string, below, will be copied here
  22.  
  23. double sdc(double radius)
  24. {
  25.    if(radius < 0.0)         // change sign of negative radius
  26.       radius = -radius;
  27.    else if(radius == 0.0)   // This is just insurance, this funtion doesn't
  28.       return(200.0);        // make sense when the radius is zero.
  29.    return CORN_SPD_CON * sqrt(radius);
  30. }
  31.  
  32. double sdw(double goal, double present)
  33. {
  34.    double ws;
  35.  
  36.    if(present > goal + 2 * SLIP_LIM)  // if too fast,
  37.       ws = present - SLIP_LIM;      // slow down.
  38.    else if(present < goal - 2 * SLIP_LIM)  // if too slow,
  39.       ws = present + SLIP_LIM;             // accelerate.
  40.    else                           // if quite close,
  41.       ws = (goal + present) / 2;      // approach desired speed gently.
  42.  
  43.    if(ws < present)          // can slip more when braking
  44.      ws -= (present - ws);
  45.  
  46.    return ws;
  47. }
  48.  
  49. double widths(double len, double rad, double wide)
  50. {
  51.    if(rad == 0.0)
  52.       return len/wide;
  53.    else if(rad > 0.0)
  54.       return (rad+.5*wide)*len/wide;
  55.    else
  56.       return (.5*wide - rad)*len/wide;
  57. }
  58.  
  59. con_vec arelys(situation s)
  60. {
  61.    const char name[] = "Arelys";      // This is the robot driver's name!
  62.    static int init_flag = 1;          // cleared by first call
  63.    double speed;                      // target speed for cornering, ft/sec
  64.    double speed_next;                 // target speed for next corner
  65.    con_vec result;                    // This is what is returned.
  66.    double width;                      // track width, feet
  67.    double alpha, vc;           // components of result
  68.    static double alpha_inc = 0.0;  // alpha increment during passing maneuver
  69.    static int counting = 0;    // will be set and counting down when passing
  70.    double dest;                // target distance from left wall
  71.    double arg;                 // for temporary storage of calculation
  72.    double widths_cur;          // length remaining of current segment, widths
  73.    static int started = 0;     // set after the "dragout"
  74.    double redline;             // speed at which to begin braking on straight
  75.    double bias;                // estimated alpha value for a curve
  76.  
  77.    if(init_flag)  {            // first time through, only copy name:
  78.       strcpy(glob_name, name);
  79.       init_flag = 0;
  80.       result.alpha = result.vc = 0;
  81.       return result;
  82.    }
  83.  
  84.   if(stuck(s.backward, s.v,s.vn, s.to_lft,s.to_rgt, &result.alpha,&result.vc))
  85.       return result;
  86.  
  87.    width = s.to_lft + s.to_rgt;
  88.    widths_cur = widths(s.to_end, s.cur_rad, width);
  89.  //  widths_nex = widths(s.nex_len, s.nex_rad, width);
  90.  
  91.    if(s.cur_rad == 0.0)
  92.       if(s.nex_rad > 0.0)
  93.          dest = 2.0 * CUSHION;
  94.       else
  95.          dest = width - 2.0 * CUSHION;
  96.    else if(s.cur_rad > 0.0)
  97.       if(s.nex_rad >= 0.0)
  98.          dest = CUSHION;
  99.       else if(widths_cur > CRIT_WIDTHS)
  100.          dest = CUSHION;
  101.       else
  102.          dest = .5 * width;
  103.    else
  104.       if(s.nex_rad <= 0.0)
  105.          dest = width - CUSHION;
  106.       else if(widths_cur > CRIT_WIDTHS)
  107.          dest = width - CUSHION;
  108.       else
  109.          dest = .5 * width;
  110.  
  111.    // calculate target speeds for current corner and the next:
  112.    if(s.cur_rad == 0.0)
  113.       arg = 0.0;
  114.    else if(s.cur_rad > 0.0)
  115.       arg = s.cur_rad + .5 * width;
  116.    else
  117.       arg = s.cur_rad - .5 * width;
  118.    speed = sdc(arg);      // speed is based on radius
  119.  
  120.    if(s.cur_rad == 0.0)     // calculate a bias for alpha in a turn:
  121.       bias = 0.0;
  122.    else if(s.cur_rad > 0.0)
  123.       bias = 100 * BIAS / speed;
  124.    else
  125.       bias = -100.0 * BIAS / speed;
  126.  
  127.    if(s.nex_rad == 0.0)     // calculate speed for next turn
  128.       arg = 0.0;
  129.    else if(s.nex_rad > 0.0)
  130.       arg = s.nex_rad + .5 * width;
  131.    else
  132.       arg = s.nex_rad - .5 * width;
  133.    speed_next = sdc(arg); // of center line of track.
  134.  
  135.    alpha = bias + STEER_GAIN * (s.to_lft - dest) / width;
  136.    alpha -= STEER_DAMP * s.vn / s.v;  // This is damping, to prevent oscillation
  137.  
  138.    // now set the tire speed, vc:
  139.    if(s.cur_rad == 0.0) {                // If we are on a straightaway,
  140.       redline = sqrt(speed_next * speed_next + BRAK_ACCEL * 2.0 * s.to_end);
  141.       if(s.v > .85 * redline)
  142.           started = 1;
  143.       if(s.v < redline)
  144.          vc = s.v + SLIP_CON / s.v;          // keep accellerating near full power
  145.       else                     // otherwise,
  146.          vc = sdw(speed_next, s.v);    // brake for next corner
  147.    }
  148.    else                              // If we're in the curve, maintain speed.
  149.       if(widths_cur > CRIT_WIDTHS)
  150.                   // if we are far from the next corner, stay at "speed".
  151.          vc = sdw(speed, s.v);
  152.       else        // but when we near the next corner, adjust to "speed_next"
  153.          vc = sdw(speed_next, s.v);
  154.    if(!started)   // don't steer during dragout
  155.       if(s.vn > .02 * s.v || s.vn < -.02 * s.v)    // except if astray
  156.          started = 1;
  157.       else
  158.          alpha = 0;
  159.    // The passing maneuver:
  160.    if(s.dead_ahead & !counting)  {  // When first encountering the car ahead:
  161.       counting = PASSING_TIME;         // setup the timer,
  162.       if(random(29999) < 15000)        // choose a right or left maneuver:
  163.          alpha_inc = SHARP_TURN;
  164.       else
  165.          alpha_inc = -SHARP_TURN;
  166.    }
  167.    if(counting)  {                // If we are still in the passing maneuver,
  168.       alpha += alpha_inc;              // change alpha
  169.       --counting;                      // count down to zero
  170.    }
  171.  
  172.    result.vc = vc;   result.alpha = alpha;
  173.    return result;
  174. }
  175.